/***************************************************************************
 创建者: 华磊
 开始时间: 2020.3.10
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef VEHICLEVELOCITYSMOOTHER_H
#define VEHICLEVELOCITYSMOOTHER_H
#include <string>
#include <vector>
#include "gSoapFoundation/soapStub.h"

//enum RobotFeedbackType
//{
//  NONE,
//  ODOMETRY,
//  COMMANDS
//};

#define PERIOD_RECORD_SIZE    5

class VehicleVelocitySmoother
{
public:
    VehicleVelocitySmoother(double deltaTimeIn,double speed_lim_xIn,double  accel_lim_xIn,
                            double speed_lim_yIn, double accel_lim_yIn,
                            double speed_lim_rotateIn, double accel_lim_rotateIn,
                            double decel_factorIn);

    void setRawVelocityCommand(VehicleVelocity msg);//ros原始速度接收
    void setAgvVelFeedback(VehicleVelocity msg);//agv反馈车速度
    void calculateSmootherVelocityCommand(VehicleVelocity &velCommandOut);

private:
     double sign(double x);
     double median(std::vector<double> values);

private:
//     RobotFeedbackType robot_feedback;  /**< What source to use as robot velocity feedback */
//     std::string name;
//     bool quiet;        /**< Quieten some warnings that are unavoidable because of velocity multiplexing. **/
     double speed_lim_x, accel_lim_x, decel_lim_x;
     double speed_lim_y, accel_lim_y, decel_lim_y;//not used
     double speed_lim_rotate, accel_lim_rotate, decel_lim_rotate;
     double decel_factor;
     double period;

     VehicleVelocity last_cmd_vel;
     VehicleVelocity  current_vel_feedback;
     VehicleVelocity   target_vel;//ros 要求

     bool                 shutdown_req; /**< Shutdown requested by nodelet; kill worker thread */
     bool                 input_active;
     double                cb_avg_time;
     struct timespec            last_cb_time;
     std::vector<double> period_record; /**< Historic of latest periods between velocity commands */
     unsigned int             pr_next; /**< Next position to fill in the periods record buffer */

//     ros::Subscriber odometry_sub;    /**< Current velocity from odometry */
//     ros::Subscriber current_vel_sub; /**< Current velocity from commands sent to the robot, not necessarily by this node */
//     ros::Subscriber raw_in_vel_sub;  /**< Incoming raw velocity commands */
//     ros::Publisher  smooth_vel_pub;  /**< Outgoing smoothed velocity commands */


};

#endif // VEHICLEVELOCITYSMOOTHER_H
